
#include "ROB_RC.h"
#include "ROB_Telemetry.h"
#include "ROB_Pantilt.h"
#include "ROB_Motor.h"

#include "ROB_IrRC.h"




void CRC::Begin(void) {
  _IRcycles = 0;
  _lastIRRemoteCmd = 0;
}



void CRC::update(void) {
  if (_IRcycles == IR_CONTROL_MAX_IDLE_CYCLES && _lastIRRemoteCmd != IR_REMOTE_BTN_NONE) {
    tmy.engineDirections = MOT_A_BREAK | MOT_B_BREAK;
    tmy.engineLPower = 0;
    tmy.engineRPower = 0;
    _lastIRRemoteCmd = IR_REMOTE_BTN_NONE;
    IR_remote_init(REMOTE_WITH_REPEAT);
    
  }
  else 
    _IRcycles++;
}




// receive Remote command
void CRC::message(uint8_t cmd, uint8_t code) {

  static int ccBtnState = 0;

  switch (cmd) {

  case RCCMD_IR_REMOTE_BTN:

    _IRcycles = 0;

    if(code == _lastIRRemoteCmd)
      break;

    if(ccBtnState) {

      switch (code) {
      case IR_REMOTE_BTN_UP:
        if(tmy.servoTiltTargetPos > SERVO_TILT_MIN_POS + SERVO_STEP)
          tmy.servoTiltTargetPos -= SERVO_STEP;
        break;
      case IR_REMOTE_BTN_DN:
        if(tmy.servoTiltTargetPos < SERVO_TILT_MAX_POS - SERVO_STEP)
          tmy.servoTiltTargetPos += SERVO_STEP;
        break;
      case IR_REMOTE_BTN_LE:
        if(tmy.servoPanTargetPos > SERVO_PAN_MIN_POS + SERVO_STEP)
          tmy.servoPanTargetPos -= SERVO_STEP;
        break;
      case IR_REMOTE_BTN_RE:
        if(tmy.servoPanTargetPos < SERVO_PAN_MAX_POS - SERVO_STEP)
          tmy.servoPanTargetPos += SERVO_STEP;
        break;
      case IR_REMOTE_BTN_CC:
        ccBtnState ^= 0x01;
        break;
      case IR_REMOTE_BTN_MM:
        handleIRRemoteBtnMM();
        break;
      }
    }
    else {

      switch (code) {
      case IR_REMOTE_BTN_UP:
        tmy.engineDirections = MOT_A_FWD | MOT_B_FWD;
        tmy.engineLPower = REMOTE_MOTOR_SPEED;
        tmy.engineRPower = REMOTE_MOTOR_SPEED;
        break;
      case IR_REMOTE_BTN_DN:
        tmy.engineDirections = MOT_A_REV | MOT_B_REV;
        tmy.engineLPower = REMOTE_MOTOR_SPEED;
        tmy.engineRPower = REMOTE_MOTOR_SPEED;
        break;
      case IR_REMOTE_BTN_LE:
        tmy.engineDirections = MOT_A_REV | MOT_B_FWD;
        tmy.engineLPower = REMOTE_MOTOR_SPEED;
        tmy.engineRPower = REMOTE_MOTOR_SPEED;
        break;
      case IR_REMOTE_BTN_RE:
        tmy.engineDirections = MOT_A_FWD | MOT_B_REV;
        tmy.engineLPower = REMOTE_MOTOR_SPEED;
        tmy.engineRPower = REMOTE_MOTOR_SPEED;
        break;
      case IR_REMOTE_BTN_CC:
        ccBtnState ^= 0x01;
        tmy.engineDirections = MOT_A_BREAK | MOT_B_BREAK;
        tmy.engineLPower = 0;
        tmy.engineRPower = 0;
        break;
      case IR_REMOTE_BTN_MM:
        handleIRRemoteBtnMM();
        tmy.engineDirections = MOT_A_BREAK | MOT_B_BREAK;
        tmy.engineLPower = 0;
        tmy.engineRPower = 0;
        break;
      }

    }
    _lastIRRemoteCmd = code;
    break;
    
    
    
    
  }
}



void CRC::handleIRRemoteBtnMM(void) {

  if(tmy.soundPlay !=6 && tmy.soundPlay != 7)
    tmy.soundPlay = 6;
  else if(tmy.soundPlay == 6)
    tmy.soundPlay = 7;
  else if(tmy.soundPlay == 7)
    tmy.soundPlay = 0;


  if(tmy.lightSystem)
    tmy.lightSystem = 0;
  else
    tmy.lightSystem = 3;

}



CRC RC;

